#include "clmotor.h"
void climotor_update_pid(void* m_ctx, float dt);

void clmotor_init(ClosedLoopMotor *m,
                  GPIO_PinName enc_a, 
                  GPIO_PinName enc_b,
                  uint16_t ppr,
                  uint32_t interval_ms,  // 0=MT模式，>0=M模式（ms）
                  float kp, float ki, float kd, float dt,
                  GPIO_PinName pwm_pin, 
                  GPIO_PinName dir_pin1, GPIO_PinName dir_pin2)
{
	// 1. 初始化测速模块（SpeedSensor 内含 Encoder）
    speed_init(&m->speed,
               enc_a, enc_b,
               ppr,
               interval_ms);

    // 2. 初始化 PID 控制器
    // dt = 0.001 表示 1ms 控制周期
    pid_init(&m->pid, kp, ki, kd, dt,
             100.0f, -100.0f,    // integral limit
             1000.0f, -1000.0f);     // output limit (-1000~1000)
	// 设置目标斜坡：每次最多增加 5 RPM
	pid_set_ramp_step(&m->pid, 5.0f);
	pid_enable_ramp(&m->pid, 1);
	
    // 3. 初始化电机驱动
    motor_init(&m->motor, dir_pin1, dir_pin2, pwm_pin);

    // 4. 默认关闭
    m->enabled = 0;
	m->update_counter = 0;
}


/*
** 在M法测速时，调用周期约为10ms
** 在MT法测速时，调用周期为5ms，但speed_update每10ms执行一次,pid每5ms执行一次
*/
void clmotor_update(ClosedLoopMotor *m) {
	m->update_counter += 1;
	
	// 1. 更新测速
	if(m->speed.interval_sec > 0 || m->update_counter >= 2) { //M法测速，或者MT法下每2次
		speed_update(&m->speed);
		m->update_counter = 0;
	}
	
	if (!m->enabled) return; //仅设置启动后运行控制
		
	// 2. 获取当前转速
	float now_rpm = speed_get_rpm(&m->speed);

	// 3. PID 控制
	float pid_out = pid_update(&m->pid, now_rpm);
	
	int8_t direction = 1;
	if(pid_out < 0) {
		direction = MOTOR_REV;
		pid_out = -pid_out;
	}
	// 4. 更新电机驱动
	motor_set(&m->motor, direction, (uint16_t)pid_out);
}
